#pragma once
#include <math.h>

namespace std_math
{
#define EPSILON 0.00000000001f

__declspec(align(16))
class CVector3
{
private:
	float _x, _y, _z;
public:
	CVector3():_x(0.0f), _y(0.0f), _z(0.0f){};
	CVector3(float vx, float vy, float vz):_x(vx), _y(vy), _z(vz){};
	CVector3(const CVector3 &v):_x(v._x), _y(v._y), _z(v._z){};
	float qs_magnitude(){return _x * _x + _y * _y + _z * _z;};
	float magnitude(){return sqrt(qs_magnitude());};
	void set(const float vx, const float vy, const float vz){_x = vx; _y = vy; _z = vz;}
	__inline float x() const {return _x;}
	__inline float y() const {return _y;}
	__inline float z() const {return _z;}
	CVector3& normalize()
	{
		float mag2 = _x * _x + _y * _y + _z * _z;
		if (fabs(mag2 - 1.0f) > EPSILON)
		{
			float mag = sqrt(mag2);
			_x /= mag;
			_y /= mag;
			_z /= mag;
		}
		return *this;
	}
	CVector3& operator+= (const CVector3& v)
	{
		this->_x += v._x;
		this->_y += v._y;
		this->_z += v._z;
		return *this;
	}
	CVector3& operator+= (const float f)
	{
		this->_x += f;
		this->_y += f;
		this->_z += f;
		return *this;
	}
	CVector3& operator-= (const CVector3& v)
	{
		this->_x -= v._x;
		this->_y -= v._y;
		this->_z -= v._z;
		return *this;
	}
	CVector3& operator-= (const float f)
	{
		this->_x -= f;
		this->_y -= f;
		this->_z -= f;
		return *this;
	}

	CVector3 operator+ (const CVector3& v) const
	{
		return (CVector3(_x + v._x, _y + v._y, _z + v._z));
	}
	CVector3 operator+ (const float f) const
	{
		return (CVector3(_x + f, _y + f, _z + f));
	}

	CVector3 operator- (const CVector3& v) const
	{
		return (CVector3(_x - v._x, _y - v._y, _z - v._z));
	}
	CVector3 operator- (const float f) const
	{
		return (CVector3(_x - f, _y - f, _z - f));
	}

	CVector3 operator* (const float& f) const
	{
		return (CVector3(_x * f, _y * f, _z * f));
	}
	__inline CVector3& operator= (const CVector3& v)
	{
		_x = v._x;
		_y = v._y;
		_z = v._z;
		return *this;
	}
	__inline CVector3& operator*= (const float& f)
	{
		this->_x *= f;
		this->_y *= f;
		this->_z *= f;
		return *this;
	}
	__inline CVector3 cross(CVector3 const& v)
	{
		  return CVector3(_y * v._z - _z * v._y, _z * v._x - _x * v._z, _x * v._y - _y * v._x);
	}
	__inline float dot(CVector3 const& v)
	{
		return _x * v._x + _y * v._y + _z * v._z;
	}
};

}